#include "modbus_interface.h"
#include "io_macro.h"
#include <rclcpp/clock.hpp>
#include <rclcpp/utilities.hpp>

namespace io_module {

ModbusInterface::ModbusInterface( 
     std::shared_ptr<rclcpp::Node>& node,
    const ModuleParam& param)
: BaseInterface(node, param) 
{
  // 初始化原子变量
  std::fill(do_bytes.begin(), do_bytes.end(), 0);
  std::fill(di_bytes.begin(), di_bytes.end(), 0);
  std::fill(ao_words.begin(), ao_words.end(), 0);
  std::fill(ai_words.begin(), ai_words.end(), 0);
}

ModbusInterface::~ModbusInterface() 
{
  // 显式释放资源
  digital_out_pub_.reset();
  digital_in_sub_.reset();
  analog_out_pub_.reset();
  analog_in_sub_.reset();
}

bool ModbusInterface::init() 
{
  try {
    // 验证参数有效性
    if(m_moduleParam.modbus_slave_name.empty()) {
      DEBUG_ERROR_OUT("Invalid Modbus slave name");
      return false;
    }

    setup_ros_communications();
    
    DEBUG_STREAM_OUT("Modbus RTU initialized for " << m_moduleParam.modbus_slave_name);
    return true;
  } catch (const std::exception& e) {
    DEBUG_ERROR_OUT("Initialization failed: " << e.what());
    return false;
  }
}

void ModbusInterface::setup_ros_communications() 
{
  auto qos = rclcpp::QoS(10);
  
  // 数字量通信
  std::string do_topic = "/modbus/device" + m_moduleParam.modbus_slave_name + "/digoutput";
  digital_out_pub_ = node_->create_publisher<std_msgs::msg::UInt8MultiArray>(
      do_topic, qos);

  std::string di_topic = "/modbus/device" + m_moduleParam.modbus_slave_name + "/diginput";
  digital_in_sub_ = node_->create_subscription<std_msgs::msg::UInt8MultiArray>(
      di_topic,
      qos,
      [this](std_msgs::msg::UInt8MultiArray::ConstSharedPtr msg) {
        digital_callback(msg);
      });

  // 模拟量通信
  std::string ao_topic = "/modbus/device" + m_moduleParam.modbus_slave_name + "/analog_output";
  analog_out_pub_ = node_->create_publisher<std_msgs::msg::UInt16MultiArray>(
    ao_topic, qos);

  std::string ai_topic = "/modbus/device" + m_moduleParam.modbus_slave_name + "/analog_input";
  analog_in_sub_ = node_->create_subscription<std_msgs::msg::UInt16MultiArray>(
      ai_topic,
      qos,
      [this](std_msgs::msg::UInt16MultiArray::ConstSharedPtr msg) {
        analog_callback(msg);
      });
}

void ModbusInterface::digital_callback(const std_msgs::msg::UInt8MultiArray::ConstSharedPtr msg) 
{
  std::lock_guard<std::mutex> lock(digital_mutex_);
  
  const size_t max_channels = std::min(msg->data.size(), di_bytes.size());
  for(size_t i = 0; i < max_channels; ++i) {
    di_bytes[i].store(msg->data[i]);
  }
  updateTime();
}

void ModbusInterface::analog_callback(const std_msgs::msg::UInt16MultiArray::ConstSharedPtr msg) 
{
  std::lock_guard<std::mutex> lock(analog_mutex_);
  
  const size_t max_channels = std::min(msg->data.size(), ai_words.size());
  for(size_t i = 0; i < max_channels; ++i) {
    ai_words[i].store(msg->data[i]);
  }
  updateTimeA();
}

bool ModbusInterface::setOutput(const uint8_t& index, const bool& level) 
{
  if(index >= m_moduleParam.num_digital_output) {
    DEBUG_ERROR_OUT("Invalid digital output index: " << index);
    return false;
  }

  const size_t channel = index / 8;
  const uint8_t bit = index % 8;

  std::lock_guard<std::mutex> lock(digital_mutex_);
  uint8_t current = do_bytes[channel].load();
  current = level ? (current | (1 << bit)) : (current & ~(1 << bit));
  do_bytes[channel].store(current);

  auto msg = std_msgs::msg::UInt8MultiArray();
  msg.data.resize(m_moduleParam.num_digital_output / 8);
  for(size_t i = 0; i < msg.data.size(); ++i) {
    msg.data[i] = do_bytes[i].load();
  }
  digital_out_pub_->publish(msg);

  DEBUG_OUT("Set DO%u to %d", index, level);
  return true;
}

bool ModbusInterface::readInput(uint64_t& rets) 
{
  std::lock_guard<std::mutex> lock(digital_mutex_);
  bool digital_timeout = dataTimeOut();
  DEBUG_WARN_ON_TRANSITION(digital_timeout, "Digital inputs data expired");
  if(digital_timeout) {
    return false;
  }

  rets = 0;
  const uint8_t channels = m_moduleParam.num_digital_input / 8;
  for(uint8_t i = 0; i < channels; ++i) {
    rets |= static_cast<uint64_t>(di_bytes[i].load()) << (i * 8);
  }
  return true;
}

bool ModbusInterface::setOutputA(const uint8_t& index, const WORD& rawData) 
{
  if(index >= m_moduleParam.num_analog_output) {
    DEBUG_ERROR_OUT("Invalid analog output index: " << index);
    return false;
  }

  std::lock_guard<std::mutex> lock(analog_mutex_);
  ao_words[index].store(rawData);

  auto msg = std_msgs::msg::UInt16MultiArray();
  msg.data.resize(m_moduleParam.num_analog_output);
  for(size_t i = 0; i < msg.data.size(); ++i) {
    msg.data[i] = ao_words[i].load();
  }
  analog_out_pub_->publish(msg);

  DEBUG_OUT("Set AO%u to %u", index, rawData);
  return true;
}

bool ModbusInterface::readInputA(std::vector<WORD>& result) 
{
  std::lock_guard<std::mutex> lock(analog_mutex_);
  bool analog_timeout = dataTimeOutA();
  DEBUG_WARN_ON_TRANSITION(analog_timeout, "Analog inputs data expired");
  if(analog_timeout) {
    return false;
  }

  result.clear();
  for(uint8_t i = 0; i < m_moduleParam.num_analog_input; ++i) {
    result.emplace_back(ai_words[i].load());
  }
  return true;
}

} // namespace io_module